#include <algorithm/pid.h>
#include <connectivity/uart.h>
#include <device/dji_motor.h>
#include <device/snail_motor.h>
#include <rtos/thread.h>
#include <clock.h>
#include <core.h>
#include <units.h>

USE_CONNECTION_GUARD;

namespace InitConnectivity {
// ----- FDCAN1 <==> wheel + feeder + cap
// ----- FDCAN2 <==> rudder + yaw
// ----- FDCAN3 <==> boardcom
/**
 * @name can错误回调函数
 * @param hfdcan fdcan句柄
 * @param _ 错误类型
 */
void CanErrorCb(FDCAN_HandleTypeDef* hfdcan, uint32_t _) {
    // ---- 处理can芯片掉电但是单片机仍在工作的情况
    // ---- 锁死程序，不断尝试重启
    if ((_ & FDCAN_IT_BUS_OFF) != RESET) {
        FDCAN_ProtocolStatusTypeDef protocolStatus = {};
        HAL_FDCAN_GetProtocolStatus(hfdcan, &protocolStatus);
        if (protocolStatus.BusOff) {
            CLEAR_BIT(hfdcan->Instance->CCCR, FDCAN_CCCR_INIT);
        }
    }
}

void InitCan() {
    HAL_FDCAN_RegisterErrorStatusCallback(os::Can<1>::instance().getHandlePtr(), CanErrorCb);
    HAL_FDCAN_RegisterErrorStatusCallback(os::Can<2>::instance().getHandlePtr(), CanErrorCb);
    HAL_FDCAN_RegisterErrorStatusCallback(os::Can<3>::instance().getHandlePtr(), CanErrorCb);
    fdCanInit(os::Can<1>::instance().getHandlePtr());
    fdCanInit(os::Can<2>::instance().getHandlePtr());
    fdCanInit(os::Can<3>::instance().getHandlePtr());
}

/**
 *
 * @tparam buffer UART接收缓冲区地址
 * @tparam size UART接收缓冲区大小
 * @param huart UART句柄
 */
template <uint8_t* buffer, size_t size>
void UartErrorHandler(UART_HandleTypeDef* huart) {
    // 校验位错误，硬件问题一般不会出现多次，忽略即可
    if (huart->ErrorCode & HAL_UART_ERROR_PE) {
        __HAL_UART_CLEAR_PEFLAG(huart);
    }
    // 帧错误
    if (huart->ErrorCode & HAL_UART_ERROR_FE) {
        __HAL_UART_CLEAR_FEFLAG(huart);
    }
    if (huart->ErrorCode & HAL_UART_ERROR_ORE) {
        __HAL_UART_CLEAR_OREFLAG(huart);
    }
    HAL_UARTEx_ReceiveToIdle_DMA(huart, buffer, size);
}

// ------ UART2 <==> referee
constexpr auto kUart2RxBufferLen = 500;
uint8_t uart2_rx_buffer[kUart2RxBufferLen];

void Uart2RxCb(UART_HandleTypeDef* huart, uint16_t len) {
    os::Uart::Instance(USART2).on_receive.emit(uart2_rx_buffer, len);
    HAL_UARTEx_ReceiveToIdle_DMA(huart, uart2_rx_buffer, kUart2RxBufferLen);
}

// ------ UART1 <==> power board
constexpr auto kUart1RxBufferLen = 20;
uint8_t uart1_rx_buffer[kUart1RxBufferLen];

void Uart1RxCb(UART_HandleTypeDef* huart, uint16_t len) {
    os::Uart::Instance(USART1).on_receive.emit(uart1_rx_buffer, len);
    HAL_UARTEx_ReceiveToIdle_DMA(huart, uart1_rx_buffer, kUart1RxBufferLen);
}

// ------ UART3 <==> dr16
constexpr auto kUart3RxBufferLen = 18;
uint8_t uart3_rx_buffer[kUart3RxBufferLen];

void Uart3RxCb(UART_HandleTypeDef* huart, uint16_t len) {
    os::Uart::Instance(USART3).on_receive.emit(uart3_rx_buffer, len);
    HAL_UARTEx_ReceiveToIdle_DMA(huart, uart3_rx_buffer, kUart3RxBufferLen);
}

void InitUart() {
    HAL_UART_RegisterCallback(os::Uart::Instance(USART2).GetHandlePtr(), HAL_UART_ERROR_CB_ID, UartErrorHandler<uart2_rx_buffer, kUart2RxBufferLen>);
    HAL_UART_RegisterRxEventCallback(os::Uart::Instance(USART2).GetHandlePtr(), Uart2RxCb);
    HAL_UARTEx_ReceiveToIdle_DMA(os::Uart::Instance(USART2).GetHandlePtr(), uart2_rx_buffer, kUart2RxBufferLen);
    HAL_UART_RegisterCallback(os::Uart::Instance(USART1).GetHandlePtr(), HAL_UART_ERROR_CB_ID, UartErrorHandler<uart1_rx_buffer, kUart1RxBufferLen>);
    HAL_UART_RegisterRxEventCallback(os::Uart::Instance(USART1).GetHandlePtr(), Uart1RxCb);
    HAL_UARTEx_ReceiveToIdle_DMA(os::Uart::Instance(USART1).GetHandlePtr(), uart1_rx_buffer, kUart1RxBufferLen);
    HAL_UART_RegisterCallback(os::Uart::Instance(USART3).GetHandlePtr(), HAL_UART_ERROR_CB_ID, UartErrorHandler<uart3_rx_buffer, kUart3RxBufferLen>);
    HAL_UART_RegisterRxEventCallback(os::Uart::Instance(USART3).GetHandlePtr(), Uart3RxCb);
    HAL_UARTEx_ReceiveToIdle_DMA(os::Uart::Instance(USART3).GetHandlePtr(), uart3_rx_buffer, kUart3RxBufferLen);
}

InitLate(do_init) {
    InitCan();
    InitUart();
}
}  // namespace InitConnectivity